#include <linux/sched.h>
#include <linux/kernel.h>
#include <linux/hdreg.h>
#include <linux/genhd.h>
#include <asm/io.h>

#define MAJOR_NR HD_MAJOR
#include "blk.h"

static inline unsigned char CMOS_READ(unsigned char addr) {
    outb_p(0x80 | addr, 0x70);
    return inb_p(0x71);
}

#define MAX_HD 2

struct hd_i_struct {
    unsigned int head, sect, cyl, wpcom, lzone, ctl;
};
struct hd_i_struct hd_info[] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
static int NR_HD = 0;

static struct hd_struct hd[MAX_HD << 6] = {
    {0, 0},
};
static int hd_blocksizes[MAX_HD << 6] = {
    0,
};

static int controller_busy(void);
static int status_ok(void);

static int controller_ready(unsigned int drive, unsigned int head) {
    int retry = 100;

    do {
        if (controller_busy() & BUSY_STAT)
            return 0;
        outb_p(0xA0 | (drive << 4) | head, HD_CURRENT);
        if (status_ok())
            return 1;
    } while (--retry);
    return 0;
}

static int status_ok(void) {
    unsigned char status = inb_p(HD_STATUS);

    if (status & BUSY_STAT)
        return 1;
    if (status & WRERR_STAT)
        return 0;
    if (!(status & READY_STAT))
        return 0;
    if (!(status & SEEK_STAT))
        return 0;
    return 1;
}

static int controller_busy(void) {
    int retries = 100000;
    unsigned char status;

    do {
        status = inb_p(HD_STATUS);
    } while ((status & BUSY_STAT) && --retries);
    return status;
}

static void hd_out(unsigned int drive, unsigned int nsect, unsigned int sect,
                   unsigned int head, unsigned int cyl, unsigned int cmd,
                   void (*intr_addr)(void)) {
    unsigned short port;

    if (drive > 1 || head > 15)
        panic("Trying to write bad sector");

    if (!controller_ready(drive, head)) {
        return;
    }
    DEVICE_INTR = intr_addr;
    outb_p(hd_info[drive].ctl, HD_CMD);
    port = HD_DATA;
    outb_p(hd_info[drive].wpcom >> 2, ++port);
    outb_p(nsect, ++port);
    outb_p(sect, ++port);
    outb_p(cyl, ++port);
    outb_p(cyl >> 8, ++port);
    outb_p(0xA0 | (drive << 4) | head, ++port);
    outb_p(cmd, ++port);
}

void unexpected_hd_interrupt(void) {
    sti();
    printk("Unexpected HD interrupt\n");
}

#define STAT_MASK (BUSY_STAT | READY_STAT | WRERR_STAT | SEEK_STAT | ERR_STAT)
#define STAT_OK   (READY_STAT | SEEK_STAT)

static void read_intr(void) {
    int i;
    int retries = 100000;

    do {
        i = (unsigned)inb_p(HD_STATUS);
        if (i & BUSY_STAT)
            continue;
        if ((i & STAT_MASK) != STAT_OK)
            break;
        if (i & DRQ_STAT)
            goto ok_to_read;
    } while (--retries > 0);
    sti();
    printk("HD: read_intr: status = 0x%02x\n", i);
    cli();
    return;
ok_to_read:
    insw(HD_DATA, REQUEST->buffer, 256);
    printk("First 16 bytes we read:\n");
    for (int j = 0; j < 16; j++)
        printk("%02x ", (unsigned char)REQUEST->buffer[j]);
    printk("\n");
    REQUEST->buffer += 512;
    REQUEST->sector++;
    i = --REQUEST->nr_sectors;
    if (i > 0) {
        DEVICE_INTR = read_intr;
        sti();
        return;
    }
}

static void do_hd_request(void) {
    unsigned int block, dev;
    unsigned int sec, head, cyl, track;
    unsigned int nsect;
    if (REQUEST && REQUEST->dev < 0)
        return;

    if (DEVICE_INTR)
        return;

    sti();
    dev = MINOR(REQUEST->dev);
    block = REQUEST->sector;
    nsect = REQUEST->nr_sectors;

    block += hd[dev].start_sect;
    dev >>= 6;
    sec = block % hd_info[dev].sect + 1;
    track = block / hd_info[dev].sect;
    head = track % hd_info[dev].head;
    cyl = track / hd_info[dev].head;
    cli();

    if (REQUEST->cmd == READ) {
        hd_out(dev, nsect, sec, head, cyl, WIN_READ, &read_intr);
        sti();
        return;
    }
    panic("unknown hd-command");
}

static void hd_geninit(void);

static struct gendisk hd_gendisk = {
    MAJOR_NR,   /* Major number */
    "hd",       /* Major name */
    hd_geninit, /* init function */
    0,          /* number */
    NULL};      /* next */

extern void hd_interrupt(void);

static void hd_geninit(void) {
    int drive, i;
    extern struct drive_info drive_info;
    unsigned char *BIOS = (unsigned char *)&drive_info;
    int cmos_disks;

    if (!NR_HD) {
        for (drive = 0; drive < 2; drive++) {
            hd_info[drive].cyl = *(unsigned short *)BIOS;
            hd_info[drive].head = *(2 + BIOS);
            hd_info[drive].wpcom = *(unsigned short *)(5 + BIOS);
            hd_info[drive].ctl = *(8 + BIOS);
            hd_info[drive].lzone = *(unsigned short *)(12 + BIOS);
            hd_info[drive].sect = *(14 + BIOS);
            BIOS += 16;
        }

        cmos_disks = CMOS_READ(0x12);
        if (cmos_disks & 0xf0) {
            if (cmos_disks & 0x0f)
                NR_HD = 2;
            else
                NR_HD = 1;
        }
    }
    i = NR_HD;
    while (i-- > 0) {
        hd[i << 6].nr_sects = 0;
        if (hd_info[i].head > 16) {
            printk("hd.c: ST-506 interface disk with more than 16 heads detected,\n");
            printk("  probably due to non-standard sector translation. Giving up.\n");
            printk("  (disk %d: cyl=%d, sect=%d, head=%d)\n", i,
                   hd_info[i].cyl,
                   hd_info[i].sect,
                   hd_info[i].head);
            if (i + 1 == NR_HD)
                NR_HD--;
            continue;
        }
        hd[i << 6].nr_sects = hd_info[i].head *
                              hd_info[i].sect * hd_info[i].cyl;
    }
    if (NR_HD) {
        set_intr_gate(0x2E, &hd_interrupt);
        outb_p(inb_p(0x21) & 0xfb, 0x21);
        outb(inb_p(0xA1) & 0xbf, 0xA1);
    }
    hd_gendisk.nr_real = NR_HD;

    for (i = 0; i < (MAX_HD << 6); i++)
        hd_blocksizes[i] = 1024;
    blksize_size[MAJOR_NR] = hd_blocksizes;

    extern struct buffer_head *free_list;
    struct buffer_head *bh = free_list;
    bh->b_dev = ROOT_DEV;
    bh->b_blocknr = 1;
    ll_rw_block(READ, 1, &bh);
}

void hd_init() {
    blk_dev[MAJOR_NR].request_fn = do_hd_request;
    hd_gendisk.next = gendisk_head;
    gendisk_head = &hd_gendisk;
}
